/***************************************************************************
 创建者: 华磊
 开始时间: 2019.5.24
 copyright: (C) 华友高科
 修改说明:
 ***************************************************************************
 *  @file xxx.h
 *  功能:
 *
 *
 *                                                                         *
 ***************************************************************************/

#ifndef PATH_3BEZIER_H
#define PATH_3BEZIER_H

#include "path.hpp"
#include "path_composite.hpp"
#include "rotational_interpolation.hpp"

namespace KDL {

typedef struct
  {
    double x, y;
  } POINT;

class Path_3Bezier : public Path
{
public:
    /**
     * @brief Path_3Bezier
     * @param startPoint 0 x ,1 y, 2 姿态
     * @param endPoint
     */
    Path_3Bezier(Vector startPoint,Vector startPoint_orient,
                 Vector endPoint,Vector endPoint_orient);

    virtual ~Path_3Bezier();
    /**
     * LengthToS() converts a physical length along the trajectory
     * to the parameter s used in Pos, Vel and Acc.  This is used because
     * in cases with large rotations the parameter s does NOT correspond to
     * the lineair length along the trajectory.
     * User should be sure that the lineair distance travelled by this
     * path object is NOT zero, when using this method !
     * (e.g. the case of only rotational change)
     * throws Error_MotionPlanning_Not_Applicable if used on composed
     * path objects.
     * @ingroup Motion
     */
    virtual double LengthToS(double length);

    /**
     * Returns the total path length of the trajectory
     * (has dimension LENGTH)
     * This is not always a physical length , ie when dealing with rotations
     * that are dominant.
     */
    virtual double PathLength();


    /**
     * Returns the Frame at the current path length s
     */
    virtual Frame Pos(double s) const;

    /**
     * Returns the velocity twist at path length s theta and with
     * derivative of s == sd
     */
    virtual Twist Vel(double s,double sd) const;

    /**
     * Returns the acceleration twist at path length s and with
     * derivative of s == sd, and 2nd derivative of s == sdd
     */
    virtual Twist Acc(double s,double sd,double sdd) const;

    /**
     * virtual constructor, constructing by copying.
     * In this case it returns the Clone() of the aggregated Path_Composite
     * because this is all one ever will need.
     */
    virtual Path* Clone();

    /**
     * Writes one of the derived objects to the stream
     */
    virtual void Write(std::ostream& os);

//    /**
//     * returns the number of underlying segments.
//     */
//    virtual int GetNrOfSegments();

//    /**
//     * returns a pointer to the underlying Path of the given segment number i.
//     * \param i segment number
//     * \return pointer to the underlying Path
//     * \warning The pointer is still owned by this class and is lifetime depends on the lifetime
//     *          of this class.
//     */
//    virtual Path* GetSegment(int i);

//    /**
//     * gets the length to the end of the given segment.
//     * \param i segment number
//     * \return length to the end of the segment, i.e. the value for s corresponding to the end of
//     *         this segment.
//     */
//    virtual double GetLengthToEndOfSegment(int i);

//    /**
//     * \param s [INPUT] path length variable for the composite.
//     * \param segment_number [OUTPUT] segments that corresponds to the path length variable s.
//     * \param inner_s [OUTPUT] path length to use within the segment.
//     */
//    virtual void GetCurrentSegmentLocation(double s, int &segment_number, double& inner_s);

    /**
     * gets an identifier indicating the type of this Path object
     */
    virtual IdentifierType getIdentifier() const {
        return (IdentifierType)100;
    }

private:

    Vector bezierPoint(double t) const;
    Vector bezierPoint(double t, Vector p0, Vector p1, Vector p2, Vector p3);
    double getBezier_length(Vector p0, Vector p1, Vector p2, Vector p3, int pointCount);

    POINT bezierPoint(double t, POINT p0, POINT p1, POINT p2, POINT p3);
    double getBezier_length(POINT p0, POINT p1, POINT p2, POINT p3, int pointCount);


private:
    Vector V_base_start;
    Vector V_base_start_orient;
    Vector V_base_end;
    Vector V_base_end_orient;
    Vector V_start_end;
    double pathlength;
    double scalelin;

};
}
#endif // PATH_3BEZIER_H
